//
// Created by kosuke on 12/23/17.
//

#include "Ukf.hpp"
#include "Eigen/Core"
#include <iostream>
#include <algorithm>
#include <math.h>
#include "global.h"

TrackingState UKF::trackingState_;


inline void cropAngle(double& a)
{
	while (a> M_PI) a -= 2.*M_PI;
	while (a<-M_PI) a += 2.*M_PI;
}

/**
* Initializes Unscented Kalman filter
*/
UKF::UKF() {

    // initial state vector
    x_merge_ = MatrixXd(5, 1);

    // initial state vector
    x_cv_ = MatrixXd(5, 1);

    // initial state vector
    x_ctrv_ = MatrixXd(5, 1);

    // initial state vector
    x_rm_ = MatrixXd(5, 1);

    // initial covariance matrix
    P_merge_ = MatrixXd(5, 5);

    // initial covariance matrix
    P_cv_ = MatrixXd(5, 5);

    // initial covariance matrix
    P_ctrv_ = MatrixXd(5, 5);

    // initial covariance matrix
    P_rm_ = MatrixXd(5, 5);

    // Process noise standard deviation longitudinal acceleration in m/s^2

    // std_a_cv_   = 0.8;
    // std_a_ctrv_ = 0.8;
    // std_a_rm_   = 5;
    // std_ctrv_yawdd_ = 0.8;
    // std_cv_yawdd_   = 0.8;
    // std_rm_yawdd_ = 3;


    // std_a_cv_   = 2;
    // std_a_ctrv_ = 2;
    // std_a_rm_   = 5;
    // std_ctrv_yawdd_ = 2;
    // std_cv_yawdd_   = 2;
    // std_rm_yawdd_ = 3;

    std_a_cv_   = 2;
    std_a_ctrv_ = 2;
    std_a_rm_   = 3;
    std_ctrv_yawdd_ = 2;
    std_cv_yawdd_   = 2;
    std_rm_yawdd_ = 3;

    // std_a_cv_   = 3;
    // std_a_ctrv_ = 3;
    // std_a_rm_   = 3;
    // std_ctrv_yawdd_ = 3;
    // std_cv_yawdd_   = 3;
    // std_rm_yawdd_ = 3;

    // ------------- not delete here
    // std_a_cv_   = 2;
    // std_a_ctrv_ = 2;
    // std_a_rm_   = 8;
    // std_ctrv_yawdd_ = 2;
    // std_cv_yawdd_   = 2;
    // std_rm_yawdd_ = 3;
    //------------------
    // Laser measurement noise standard deviation position1 in m
    std_laspx_ = 0.15;
    // std_laspx_ = 0.3;
    // Laser measurement noise standard deviation position2 in m
    std_laspy_ = 0.15;
    // std_laspy_ = 0.3;

    // initially set to false, set to true in first call of ProcessMeasurement
    is_initialized_ = false;

	
	// time when the state is true, in us
    time_us_ = 0.0;

    // state dimension
    n_x_ = 5;

    // Augmented state dimension
    n_aug_ = 7;

    // Sigma point spreading parameter
    lambda_ = 3 - n_x_;

    // Augmented sigma point spreading parameter
    lambda_aug_ = 3 - n_aug_;

    // predicted sigma points matrix
//    Xsig_pred_ = MatrixXd(n_x_, 2 * n_aug_ + 1);

    // predicted sigma points matrix
    Xsig_pred_cv_ = MatrixXd(n_x_, 2 * n_aug_ + 1);

    // predicted sigma points matrix
    Xsig_pred_ctrv_ = MatrixXd(n_x_, 2 * n_aug_ + 1);

    // predicted sigma points matrix
    Xsig_pred_rm_ = MatrixXd(n_x_, 2 * n_aug_ + 1);

    //create vector for weights
    weights_ = VectorXd(2 * n_aug_ + 1);

    // the current NIS for laser
    NIS_laser_ = 0.0;

    // different from paper, might be wrong
    p1_.push_back(0.9);
    p1_.push_back(0.05);
    p1_.push_back(0.05);

    p2_.push_back(0.05);
    p2_.push_back(0.9);
    p2_.push_back(0.05);

    p3_.push_back(0.05);
    p3_.push_back(0.05);
    p3_.push_back(0.9);

    // p1_.push_back(0.8);
    // p1_.push_back(0.1);
    // p1_.push_back(0.1);

    // p2_.push_back(0.8);
    // p2_.push_back(0.1);
    // p2_.push_back(0.1);

    // p3_.push_back(0.1);
    // p3_.push_back(0.1);
    // p3_.push_back(0.8);

    modeMatchProbCV2CV_ = 0;
    modeMatchProbCTRV2CV_ = 0;
    modeMatchProbRM2CV_ = 0;

    modeMatchProbCV2CTRV_ = 0;
    modeMatchProbCTRV2CTRV_ = 0;
    modeMatchProbRM2CTRV_ = 0;

    modeMatchProbCV2RM_ = 0;
    modeMatchProbCTRV2RM_ = 0;
    modeMatchProbRM2RM_ = 0;

    modeProbCV_ = 0.33;
    modeProbCTRV_ = 0.33;
    modeProbRM_ = 0.33;

    zPredCVl_ = VectorXd(2);
    zPredCTRVl_ =  VectorXd(2);
    zPredRMl_ =  VectorXd(2);

    zPredCVr_ = VectorXd(3);
    zPredCTRVr_ =  VectorXd(3);
    zPredRMr_ =  VectorXd(3);

//    lS_ = MatrixXd(2,2);
//    rS_ = MatrixXd(3,3);
    lS_cv_   = MatrixXd(2,2);
    lS_ctrv_ = MatrixXd(2,2);
    lS_rm_   = MatrixXd(2,2);

    rS_cv_   = MatrixXd(3,3);
    rS_ctrv_   = MatrixXd(3,3);
    rS_rm_   = MatrixXd(3,3);

    K_cv_   = MatrixXd(2,2);
    K_ctrv_ = MatrixXd(2,2);
    K_rm_   = MatrixXd(2,2);

    gammaG_ = 9.21;
    pD_     = 0.9;
    pG_     = 0.99;

    //track parameter
	state_ = 1;
    x_merge_yaw_ = 0;


//    double gammaG = 4.61; // 90%
//    double gammaG = 5.99; // 95%
//    double gammaG = 7.38; // 97.5%
//    gammaG_ = 9.21; // 99% global variable
}


UKF::UKF(const UkfParams& params) {

    // initial state vector
    x_merge_ = MatrixXd(5, 1);

    // initial state vector
    x_cv_ = MatrixXd(5, 1);

    // initial state vector
    x_ctrv_ = MatrixXd(5, 1);

    // initial state vector
    x_rm_ = MatrixXd(5, 1);

    // initial covariance matrix
    P_merge_ = MatrixXd(5, 5);

    Q_ = MatrixXd(5, 5);

    // initial covariance matrix
    P_cv_ = MatrixXd(5, 5);

    // initial covariance matrix
    P_ctrv_ = MatrixXd(5, 5);

    // initial covariance matrix
    P_rm_ = MatrixXd(5, 5);


    std_a_cv_   = params.std_a[0];
    std_a_ctrv_ = params.std_a[1];
    std_a_rm_   = params.std_a[2];

    std_ctrv_yawdd_ = params.std_yawdd[0];
    std_cv_yawdd_   = params.std_yawdd[1];
    std_rm_yawdd_   = params.std_yawdd[2];

    // Laser measurement noise standard deviation position1 in m
    std_laspx_ = params.std_lidar[0];
    std_laspy_ = params.std_lidar[1];

    // initially set to false, set to true in first call of ProcessMeasurement
    is_initialized_ = false;


    // state dimension
    n_x_ = 5;

    // Augmented state dimension
    n_aug_ = 7;

    // Sigma point spreading parameter
    lambda_ = 3 - n_x_;

    // Augmented sigma point spreading parameter
    lambda_aug_ = 3 - n_aug_;

    // predicted sigma points matrix
    Xsig_pred_cv_ = MatrixXd(n_x_, 2 * n_aug_ + 1);

    // predicted sigma points matrix
    Xsig_pred_ctrv_ = MatrixXd(n_x_, 2 * n_aug_ + 1);

    // predicted sigma points matrix
    Xsig_pred_rm_ = MatrixXd(n_x_, 2 * n_aug_ + 1);

    //create vector for weights
    weights_ = VectorXd(2 * n_aug_ + 1);

    // the current NIS for laser
    NIS_laser_ = 0.0;

    // for interaction
	for(int i=0; i<3; i++) p1_.emplace_back(params.p123[i]);
	
	for(int i=0; i<3; i++) p2_.emplace_back(params.p123[i+3]);

	for(int i=0; i<3; i++) p3_.emplace_back(params.p123[i+6]);

    modeMatchProbCV2CV_ = 0;
    modeMatchProbCTRV2CV_ = 0;
    modeMatchProbRM2CV_ = 0;

    modeMatchProbCV2CTRV_ = 0;
    modeMatchProbCTRV2CTRV_ = 0;
    modeMatchProbRM2CTRV_ = 0;

    modeMatchProbCV2RM_ = 0;
    modeMatchProbCTRV2RM_ = 0;
    modeMatchProbRM2RM_ = 0;

    modeProbCV_ = 0.33;
    modeProbCTRV_ = 0.33;
    modeProbRM_ = 0.33;

    zPredCVl_ = VectorXd(2);
    zPredCTRVl_ =  VectorXd(2);
    zPredRMl_ =  VectorXd(2);

    zPredCVr_ = VectorXd(3);
    zPredCTRVr_ =  VectorXd(3);
    zPredRMr_ =  VectorXd(3);

    lS_cv_   = MatrixXd(2,2);
    lS_ctrv_ = MatrixXd(2,2);
    lS_rm_   = MatrixXd(2,2);

    rS_cv_   = MatrixXd(3,3);
    rS_ctrv_   = MatrixXd(3,3);
    rS_rm_   = MatrixXd(3,3);

    K_cv_   = MatrixXd(2,2);
    K_ctrv_ = MatrixXd(2,2);
    K_rm_   = MatrixXd(2,2);

    gammaG_ = 9.21;
    pD_     = 0.9;
    pG_     = 0.99;

    //track parameter
	state_ = 0;
	life_time_ = 0;
    x_merge_yaw_ = 0;
	
	init_skip_ = params.initSkip;
	init_v_thresh_ = params.initVThresh;
	init_yaw_thresh_ = params.initYawThresh;
	v_static_thresh_ = params.vStaticThresh;
	yaw_static_thresh_ = params.yawStaticThresh;

    pred_dt_ = params.predDeltaTime;
	updateBBLastTime[0] = 0;
	updateBBLastTime[1] = 0;
}

const double UKF::box_limit_xyz_[3] = {8, 3, 3};

UKF::~UKF() {}

Vector3d UKF::getXYZLength(const pcl::PointCloud<pcl::PointXYZ>& box, const VectorXd& v, int debug)
{
	Vector3d xyz;
	xyz(2) = abs(box[4].z - box[0].z);

	Vector2d axis1, axis2;

	axis1 << box[0].x - box[1].x, box[0].y - box[1].y;
	axis2 << box[0].x - box[3].x, box[0].y - box[3].y;
	double axis1_len = axis1.norm();
	double axis2_len = axis2.norm();
	double v_len = v.norm();

	double tmp1 = axis1.dot(v) / axis1_len / v_len;
	double tmp2 = axis2.dot(v) / axis2_len / v_len;
	
	if(abs(tmp1) > abs(tmp2)){
		xyz(0) = axis1_len;
 		xyz(1) = axis2_len;
	}else{
		xyz(0) = axis2_len;
		xyz(1) = axis1_len;
	}
	
	//if(debug == 1){
		//ROS_DEBUG_NAMED("updateBB", "target %d getxyz v(%f, %f) axix1(%f, %f) axis2(%f, %f)", box_id_, v(0), v(1), axis1(0), axis1(1), axis2(0), axis2(1));
	//}
    return xyz;
}

Vector3d UKF::getMedianXYZ(const vector<Vector3d>& vec)
{
	vector<Vector3d> tmp(vec);
	Vector3d res;
	for(int i=0; i<3; i++)
	{
		sort(tmp.begin(), tmp.end(), [i](const Vector3d& a, const Vector3d& b) -> bool{
				return a(i) < b(i);
		});
		res(i) = tmp[tmp.size() / 2](i);
		if( res(i) > box_limit_xyz_[i]) 	res(i) = box_limit_xyz_[i];
	}
	return res;	
}


void UKF::initializeForEgo(const Vector3d& z, double timestamp) {
    // ego input: [x, y, yaw]
	
	state_ ++;
	if(state_ == 1){
		init_cp_.emplace_back(z.head<2>());
		last_time_ = timestamp;
		return;
	}

	double dt = timestamp - last_time_;
	if(dt < 0.1) 	return;
	
	last_time_ = timestamp;
	VectorXd last_z = init_cp_.front();
	VectorXd v = (z.head<2>() - last_z) / dt;
    ROS_DEBUG_NAMED("ego", "vyaw(%f), ayaw(%f)", atan2(v(1), v(0)), z(2));
	
	// ROS_DEBUG_NAMED("ego", "init time(%lf, %lf), dt(%lf), v(%f, %f) yaw(%f)", last_time_, timestamp, dt, v(0), v(1), init_yaw_.back());
	// first measurement x_merge_ [x, y, v, yaw, w]
	x_merge_ << z(0), z(1), v.norm(), z(2), 0;
    P_merge_ <<   0.5,    0, 0, 0, 0,
                    0,  0.5, 0, 0, 0,
                    0,    0, 10, 0, 0,
                    0,    0, 0, 2, 0,
                    0,    0, 0, 0, 6;
    
    Q_ <<   0.0,    0, 0, 0, 0,
            0,  0.0, 0, 0, 0,
            0,    0, 0, 0, 0,
            0,    0, 0, 0, 0,
            0,    0, 0, 0, 0;

	// set weights
	double weight_0 = lambda_aug_ / (lambda_aug_ + n_aug_);
	weights_(0) = weight_0;
	for (int i = 1; i < 2 * n_aug_ + 1; i++) {  //2n+1 weights
		double weight = 0.5 / (n_aug_ + lambda_aug_);
		weights_(i) = weight;
	}

	zPredCVl_(0) = z(0);
	zPredCVl_(1) = z(1);

	zPredCTRVl_(0) = z(0);
	zPredCTRVl_(1) = z(1);

	zPredRMl_(0) = z(0);
	zPredRMl_(1) = z(1);


	x_cv_ = x_ctrv_ = x_rm_ = x_merge_;
	P_cv_ = P_ctrv_ = P_rm_ = P_merge_;

    lS_cv_   <<  1, 0,
                0, 1;
    lS_ctrv_ <<  1, 0,
                0, 1;
    lS_rm_   <<  1, 0,
                0, 1;

    std_laspx_ = 5e-2;
    std_laspy_ = 5e-2;

	is_initialized_ = true;
}


void UKF::initialize(const pcl::PointCloud<pcl::PointXYZ>& box, const VectorXd& z, double timestamp) {
	
	init_cp_.emplace_back(z);
	init_boxes_.emplace_back(box);
	init_time_.emplace_back(timestamp);
	state_ ++;
	
	if(state_ == 1){
		last_time_ = timestamp;
		init_z_cp_ = (box[0].z + box[4].z) / 2;
	}else{
		double dt = timestamp - last_time_;
		last_time_ = timestamp;
		if(init_cp_.size() > init_skip_){
			// init_cp_ size = skip + 1	
			// init_v_ = 1
			dt = timestamp - *(init_time_.end() -1 -init_skip_);
			VectorXd last_z = *(init_cp_.end() - 1 - init_skip_);
			VectorXd v = (z - last_z) / dt;
			init_v_.emplace_back(v.norm());
			init_yaw_.emplace_back(sin(atan2(v(1), v(0))));
			init_box_xyz_.emplace_back(getXYZLength(*(init_boxes_.end()-2), v));
			//cout << "z: " << z.transpose() << endl;
			//cout << "lastz: " << last_z.transpose() << endl;
			ROS_DEBUG_NAMED("ukf_init", "[ukf_init] target %d init v(%f, %f) dt(%f)", box_id_, v(0), v(1), dt);
		}
		// once we get v dir, we can imply axis x and y
		//if(init_box_xyz_.empty()){
			//init_box_xyz_.emplace_back(getXYZLength(init_boxes_[0], v));	
		//}
		//Vector3d tmp = init_box_xyz_.back();
		//ROS_DEBUG_NAMED("ukf", "target %d init_box_xyz_(%f, %f, %f)", box_id_, tmp(0), tmp(1), tmp(2));
	}
	
	// at least got 4 init_v_ size
	assert(trackingState_.STABLE >= 3+init_skip_);
	if(state_ == trackingState_.STABLE - 1)
	{
		// evaluate the v and yaw
		int n = init_v_.size();
		double mean_v = std::accumulate(init_v_.begin(), init_v_.end(), 0.0) / n;
		double mean_yaw = std::accumulate(init_yaw_.begin(), init_yaw_.end(), 0.0) / n;
			
		double total_v = 0;
		double total_yaw = 0;
		for(int i=0; i<n; i++)
		{
			double tmp_v = init_v_[i] - mean_v;
			tmp_v *= tmp_v;
			total_v += tmp_v;
			double tmp_yaw = init_yaw_[i] - mean_yaw;
			cropAngle(tmp_yaw);

			tmp_yaw *= tmp_yaw;
			total_yaw += tmp_yaw;
			if((tmp_v > init_v_thresh_*init_v_thresh_) || (tmp_yaw > init_yaw_thresh_*init_yaw_thresh_))
			{
				ROS_DEBUG_NAMED("ukf_init", "[ukf_init] target %d initdone but one of v(%f) or yaw(%f) differ from mean a lot!!", box_id_, tmp_v, tmp_yaw);
				ROS_DEBUG_NAMED("ukf_init", "[ukf_init] mean v(%f) yaw(%f)", mean_v, mean_yaw);
				_printVecForDebug("ukf_init", init_v_);
				_printVecForDebug("ukf_init", init_yaw_);
				state_ = trackingState_.LOST;
				return;
			}
		}

		// judge is static or not
		if((total_v/n < v_static_thresh_*v_static_thresh_) && (total_yaw/n < yaw_static_thresh_*yaw_static_thresh_))
		{
			ROS_DEBUG_NAMED("ukf_init", "[ukf_init] target %d is static for mean diff v(%f) and yaw(%f) are too small", box_id_, total_v/n, total_yaw/n);
			state_ = trackingState_.ISSTATIC;
			return;
		}
		
		// first measurement x_merge_ [x, y, v, yaw, w]
		x_merge_ << z(0), z(1), init_v_.back(), init_yaw_.back(), 0.1;
		P_merge_ <<   0.5,    0, 0, 0, 0,
						0,  0.5, 0, 0, 0,
						0,    0, 5, 0, 0,
						0,    0, 0, 2, 0,
						0,    0, 0, 0, 6;
        
        Q_.fill(0.0);

		// set weights
		double weight_0 = lambda_aug_ / (lambda_aug_ + n_aug_);
		weights_(0) = weight_0;
		for (int i = 1; i < 2 * n_aug_ + 1; i++) {  //2n+1 weights
			double weight = 0.5 / (n_aug_ + lambda_aug_);
			weights_(i) = weight;
		}

		zPredCVl_(0) = z(0);
		zPredCVl_(1) = z(1);

		zPredCTRVl_(0) = z(0);
		zPredCTRVl_(1) = z(1);

		zPredRMl_(0) = z(0);
		zPredRMl_(1) = z(1);


		x_cv_ = x_ctrv_ = x_rm_ = x_merge_;
		P_cv_ = P_ctrv_ = P_rm_ = P_merge_;

		lS_cv_   <<  1, 0,
					 0, 1;
		lS_ctrv_ <<  1, 0,
					 0, 1;
		lS_rm_   <<  1, 0,
					 0, 1;

		is_initialized_ = true;
		//ROS_DEBUG_NAMED("ukf_init", "target %d is initialized!!", box_id_);
		box_color_.setOnes();
		box_color_ = (box_color_ + Vector3d::Random()) / 2;
		// init BBox: pick an bbox with proper area
		Bbox_XYZ_ = getMedianXYZ(init_box_xyz_);
		//ROS_DEBUG_NAMED("ukf", "target %d is initialized Bbox_XYZ_(%f, %f, %f)", box_id_, Bbox_XYZ_(0), Bbox_XYZ_(1), Bbox_XYZ_(2));
	}

}

double UKF::CalculateGauss(VectorXd z, int sensorInd, int modelInd){
    if(sensorInd == 0){
        if      (modelInd == 0) {
            double  detS = fabs(lS_cv_.determinant());
            MatrixXd inS = lS_cv_.inverse();
//            cout << z << endl << zPredCVl_ << endl;
//            VectorXd s = (z-zPredCVl_).transpose();
//            double a = ((z-zPredCVl_).transpose()*inS*(z-zPredCVl_));
            return exp(-1*(((z-zPredCVl_).transpose()*inS*(z-zPredCVl_))(0))/2)/sqrt(((2*M_PI)*(2*M_PI)*detS));
        }
        else if (modelInd == 1) {
            double  detS = fabs(lS_ctrv_.determinant());
            MatrixXd inS = lS_ctrv_.inverse();
            return exp(-1*(((z-zPredCTRVl_).transpose()*inS*(z-zPredCTRVl_))(0))/2)/sqrt(((2*M_PI)*(2*M_PI)*detS));
        }
        else                    {
            double  detS = fabs(lS_rm_.determinant());
            MatrixXd inS = lS_rm_.inverse();
            return exp(-1*(((z-zPredRMl_).transpose()  *inS*(z-zPredRMl_))(0))/2)  /sqrt(((2*M_PI)*(2*M_PI)*detS));
        }
    }
    else if(sensorInd == 1){
        if (modelInd == 0){
            double  detS = fabs(rS_cv_.determinant());
            MatrixXd inS = rS_cv_.inverse();
            double cvProb = exp(-1*(((z-zPredCVr_).transpose()  *inS*(z-zPredCVr_))(0))/2)  /sqrt((2*M_PI)*(2*M_PI)*(2*M_PI)*detS);
            if(cvProb != 0) return cvProb;
            else {
                z[1] = -1 * z[1];
                cvProb = exp(-1*(((z-zPredCVr_).transpose()  *inS*(z-zPredCVr_))(0))/2)  /sqrt((2*M_PI)*(2*M_PI)*(2*M_PI)*detS);
                return cvProb;
            }
        }
        else if (modelInd == 1) {
            double detS = fabs(rS_ctrv_.determinant());
            MatrixXd inS = rS_ctrv_.inverse();
            double ctrvProb = exp(-1 * (((z - zPredCTRVr_).transpose() * inS * (z - zPredCTRVr_))(0)) / 2) /
                              sqrt((2 * M_PI) * (2 * M_PI) * (2 * M_PI) * detS);
            if (ctrvProb != 0) return ctrvProb;
            else {
                z[1] = -1 * z[1];
                ctrvProb = exp(-1 * (((z - zPredCTRVr_).transpose() * inS * (z - zPredCTRVr_))(0)) / 2) /
                           sqrt((2 * M_PI) * (2 * M_PI) * (2 * M_PI) * detS);
                return ctrvProb;
            }
        }
        else    {
            double  detS = fabs(rS_rm_.determinant());
            MatrixXd inS = rS_rm_.inverse();
            double rmProb = exp(-1*(((z-zPredRMr_).transpose()  *inS*(z-zPredRMr_))(0))/2)  /sqrt((2*M_PI)*(2*M_PI)*(2*M_PI)*detS);
            if(rmProb != 0)return rmProb;
            else{
                z[1] = -1 * z[1];
                rmProb = exp(-1*(((z-zPredRMr_).transpose()  *inS*(z-zPredRMr_))(0))/2)  /sqrt((2*M_PI)*(2*M_PI)*(2*M_PI)*detS);
                return rmProb;
            }
        }
    }
}

void UKF::UpdateModeProb(vector<double> lambdaVec){
    double cvGauss   = lambdaVec[0];
    double ctrvGauss = lambdaVec[1];
    double rmGauss   = lambdaVec[2];
    double sumGauss  = cvGauss*modeProbCV_ + ctrvGauss*modeProbCTRV_ + rmGauss*modeProbRM_;

    modeProbCV_   = (cvGauss  *modeProbCV_)  /sumGauss;
    modeProbCTRV_ = (ctrvGauss*modeProbCTRV_)/sumGauss;
    modeProbRM_   = (rmGauss  *modeProbRM_)  /sumGauss;
    if(fabs(modeProbCV_)   < 0.0001) modeProbCV_   = 0.0001;
    if(fabs(modeProbCTRV_) < 0.0001) modeProbCTRV_ = 0.0001;
    if(fabs(modeProbRM_)   < 0.0001) modeProbRM_   = 0.0001;

    // cout << endl<<"mode prob"<<endl<<"cv: "<<modeProbCV_<<endl<<"ctrv: "<<modeProbCTRV_<<endl<<"rm: "<<modeProbRM_<<endl;
}

void UKF::UpdateYawWithHighProb(){
    if(modeProbCV_ > modeProbCTRV_){
        if(modeProbCV_ > modeProbRM_){
            x_merge_yaw_ = x_cv_(3);
        }
        else{
            x_merge_yaw_ = x_rm_(3);
        }
    }
    else{
        if(modeProbCTRV_ > modeProbRM_){
            x_merge_yaw_ = x_ctrv_(3);
        }
        else{
            x_merge_yaw_ = x_rm_(3);
        }
    }
    x_merge_(3) = x_merge_yaw_;
}

void UKF::MergeEstimationAndCovariance(){
    // cout << endl<<"merge x cv" <<endl << x_cv_ <<endl;
    // cout << endl<<"merge x ctrv" <<endl << x_ctrv_ <<endl;
    // cout << endl<<"merge x rm" <<endl << x_rm_ <<endl;


    x_merge_ = modeProbCV_*x_cv_ + modeProbCTRV_ *x_ctrv_ + modeProbRM_ * x_rm_;
    UpdateYawWithHighProb();
    
    while (x_merge_(3)> M_PI) x_merge_(3) -= 2.*M_PI;
    while (x_merge_(3)<-M_PI) x_merge_(3) += 2.*M_PI;

    P_merge_ = modeProbCV_  *(P_cv_   +(x_cv_   - x_merge_)*(x_cv_   - x_merge_).transpose()) +
               modeProbCTRV_*(P_ctrv_ +(x_ctrv_ - x_merge_)*(x_ctrv_ - x_merge_).transpose()) +
               modeProbRM_  *(P_rm_   +(x_rm_   - x_merge_)*(x_rm_   - x_merge_).transpose());

}

void UKF::MixingProbability() {

    // p1_[0] actually means p11, which means the probability of mode CV transfering to CV
    // p2_[0] means p21, meaning that the pro of mode CTRV transfering to CV

    double sumProb1 = modeProbCV_*p1_[0]+modeProbCTRV_*p2_[0]+modeProbRM_*p3_[0];
    double sumProb2 = modeProbCV_*p1_[1]+modeProbCTRV_*p2_[1]+modeProbRM_*p3_[1];
    double sumProb3 = modeProbCV_*p1_[2]+modeProbCTRV_*p2_[2]+modeProbRM_*p3_[2];

    // corresponding to paper \mu_{i, j} meaning mode i transfering to j
    modeMatchProbCV2CV_     = modeProbCV_  *p1_[0]/sumProb1;
    modeMatchProbCTRV2CV_   = modeProbCTRV_*p2_[0]/sumProb1;
    modeMatchProbRM2CV_     = modeProbRM_  *p3_[0]/sumProb1;

    modeMatchProbCV2CTRV_   = modeProbCV_  *p1_[1]/sumProb2;
    modeMatchProbCTRV2CTRV_ = modeProbCTRV_*p2_[1]/sumProb2;
    modeMatchProbRM2CTRV_   = modeProbRM_  *p3_[1]/sumProb2;

    modeMatchProbCV2RM_     = modeProbCV_  *p1_[2]/sumProb3;
    modeMatchProbCTRV2RM_   = modeProbCTRV_*p2_[2]/sumProb3;
    modeMatchProbRM2RM_     = modeProbRM_  *p3_[2]/sumProb3;

}


void UKF::Interaction() {

    MatrixXd x_pre_cv   = x_cv_;
    MatrixXd x_pre_ctrv = x_ctrv_;
    MatrixXd x_pre_rm   = x_rm_;
    MatrixXd P_pre_cv   = P_cv_;
    MatrixXd P_pre_ctrv = P_ctrv_;
    MatrixXd P_pre_rm   = P_rm_;
    x_cv_   = modeMatchProbCV2CV_  *x_pre_cv + modeMatchProbCTRV2CV_  *x_pre_ctrv + modeMatchProbRM2CV_  *x_pre_rm;
    x_ctrv_ = modeMatchProbCV2CTRV_*x_pre_cv + modeMatchProbCTRV2CTRV_*x_pre_ctrv + modeMatchProbRM2CTRV_*x_pre_rm;
    x_rm_   = modeMatchProbCV2RM_  *x_pre_cv + modeMatchProbCTRV2RM_  *x_pre_ctrv + modeMatchProbRM2RM_*x_pre_rm;

    // not interacting yaw(-pi ~ pi)
    x_cv_(3)   = x_pre_cv(3);
    x_ctrv_(3) = x_pre_ctrv(3);
    x_rm_(3)   = x_pre_rm(3);

	ROS_DEBUG_NAMED("inter", "(inter, %d, %f, %f)", 0, x_cv_(0), x_cv_(1));
	ROS_DEBUG_NAMED("inter", "(inter, %d, %f, %f)", 1, x_ctrv_(0), x_ctrv_(1));
	ROS_DEBUG_NAMED("inter", "(inter, %d, %f, %f)", 2, x_rm_(0), x_rm_(1));
//    cout<< "cv x state before interaction: "  <<endl<<x_pre_cv<<endl;
//    cout<< "ctrv x state before interaction: "<<endl<<x_pre_ctrv<<endl;
//    cout<< "rm x state before interaction: "  <<endl<<x_pre_rm<<endl<<endl;
//    cout<< "rm match prob: "<< endl << modeMatchProbCV2RM_ << " "<<modeMatchProbCTRV2RM_<<" "<<modeMatchProbRM2RM_<<endl<<endl;
//    cout<< "cv x state after interaction: "  <<endl<<x_cv_<<endl;
//    cout<< "ctrv x state after interaction: "<<endl<<x_ctrv_<<endl;
//    cout<< "rm x state after interaction: "  <<endl<<x_rm_<<endl<<endl;
    // normalizing angle
    while (x_cv_(3)  > M_PI) x_cv_(3)   -= 2.*M_PI;
    while (x_cv_(3)  <-M_PI) x_cv_(3)   += 2.*M_PI;
    while (x_ctrv_(3)> M_PI) x_ctrv_(3) -= 2.*M_PI;
    while (x_ctrv_(3)<-M_PI) x_ctrv_(3) += 2.*M_PI;
    while (x_rm_(3)  > M_PI) x_rm_(3)   -= 2.*M_PI;
    while (x_rm_(3)  <-M_PI) x_rm_(3)   += 2.*M_PI;

    P_cv_   = modeMatchProbCV2CV_    *(P_pre_cv  +(x_pre_cv -  x_cv_)*   (x_pre_cv -  x_cv_).transpose()) +
              modeMatchProbCTRV2CV_  *(P_pre_ctrv+(x_pre_ctrv -x_cv_)*   (x_pre_ctrv -x_cv_).transpose())+
              modeMatchProbRM2CV_    *(P_pre_rm + (x_pre_rm -  x_cv_)*   (x_pre_rm -  x_cv_).transpose());
    P_ctrv_ = modeMatchProbCV2CTRV_  *(P_pre_cv+  (x_pre_cv -  x_ctrv_)* (x_pre_cv-   x_ctrv_).transpose()) +
              modeMatchProbCTRV2CTRV_*(P_pre_ctrv+(x_pre_ctrv -x_ctrv_)* (x_pre_ctrv -x_ctrv_).transpose())+
              modeMatchProbRM2CTRV_  *(P_pre_rm + (x_pre_rm -  x_ctrv_)* (x_pre_rm -  x_ctrv_).transpose());
    P_rm_   = modeMatchProbCV2RM_    *(P_pre_cv+  (x_pre_cv -  x_rm_)*   (x_pre_cv -  x_rm_).transpose()) +
              modeMatchProbCTRV2RM_  *(P_pre_ctrv+(x_pre_ctrv -x_rm_)*   (x_pre_ctrv -x_rm_).transpose())+
              modeMatchProbRM2RM_    *(P_pre_rm + (x_pre_rm -  x_rm_)*   (x_pre_rm -  x_rm_).transpose());

}


/**
* @param {MeasurementPackage} meas_package The latest measurement data of
* either radar or laser.
*/
void UKF::ProcessIMMUKF(double timestamp, bool flag) {

	double dt = timestamp - last_time_;
	last_time_ = timestamp;
	
    /*****************************************************************************
    *  IMM Mixing and Interaction
    ****************************************************************************/
    if(flag)
    {
        MixingProbability();
        Interaction();
    }
    /*****************************************************************************
    *  Prediction
    ****************************************************************************/
    Prediction(dt, 0);
    Prediction(dt, 1);
    Prediction(dt, 2);

    /*****************************************************************************
    *  Update
    ****************************************************************************/
    UpdateLidar(0);
    UpdateLidar(1);
    UpdateLidar(2);

}

void UKF::PostProcessIMMUKF(vector<double> lambdaVec) {
    /*****************************************************************************
    *  IMM Merge Step
    ****************************************************************************/
    UpdateModeProb(lambdaVec);
    MergeEstimationAndCovariance();
}



void UKF::Ctrv(double p_x, double p_y, double v, double yaw, double yawd, double nu_a, double nu_yawdd,
               double delta_t, vector<double>&state) {
    //predicted state values
    double px_p, py_p;

    //avoid division by zero
    if (fabs(yawd) > 0.001) {
        px_p = p_x + v / yawd * (sin(yaw + yawd * delta_t) - sin(yaw));
        py_p = p_y + v / yawd * (cos(yaw) - cos(yaw + yawd * delta_t));
    }
    else {
        px_p = p_x + v * delta_t * cos(yaw);
        py_p = p_y + v * delta_t * sin(yaw);
    }
    double v_p = v;
    double yaw_p = yaw + yawd * delta_t;
    double yawd_p = yawd;

    //add noise
    px_p = px_p + 0.5 * nu_a * delta_t * delta_t * cos(yaw);
    py_p = py_p + 0.5 * nu_a * delta_t * delta_t * sin(yaw);
    v_p = v_p + nu_a*delta_t;

    yaw_p = yaw_p + 0.5*nu_yawdd*delta_t*delta_t;
    yawd_p = yawd_p + nu_yawdd*delta_t;


    state[0] = px_p;
    state[1] = py_p;
    state[2] = v_p;
    state[3] = yaw_p;
    state[4] = yawd_p;
}

void UKF::Cv(double p_x, double p_y, double v, double yaw, double yawd, double nu_a, double nu_yawdd,
             double delta_t, vector<double>&state) {
    //predicted state values
    double px_p = p_x + v*cos(yaw)*delta_t;
    double py_p = p_y + v*sin(yaw)*delta_t;

    double v_p = v;
    // not sure which one, works better in curve by using yaw
    double yaw_p = yaw;
//    double yaw_p = 0;
    double yawd_p = yawd;

    //add noise
    px_p = px_p + 0.5 * nu_a * delta_t * delta_t * cos(yaw);
    py_p = py_p + 0.5 * nu_a * delta_t * delta_t * sin(yaw);
    v_p = v_p + nu_a*delta_t;

    yaw_p = yaw_p + 0.5*nu_yawdd*delta_t*delta_t;
    yawd_p = yawd_p + nu_yawdd*delta_t;

    state[0] = px_p;
    state[1] = py_p;
    state[2] = v_p;
    state[3] = yaw_p;
    state[4] = yawd_p;

}


void UKF::randomMotion(double p_x, double p_y, double v, double yaw, double yawd, double nu_a, double nu_yawdd,
                       double delta_t, vector<double>&state) {
    // double px_p   = p_x + 0.5 * nu_a * delta_t * delta_t * cos(yaw);
    // double py_p   = p_y + 0.5 * nu_a * delta_t * delta_t * sin(yaw);
    // double v_p    = v   + nu_a*delta_t;

    // double yaw_p  = yaw  + 0.5*nu_yawdd*delta_t*delta_t;
    // double yawd_p = yawd + nu_yawdd*delta_t;

    double px_p   = p_x;
    double py_p   = p_y;
    double v_p    = v;

    double yaw_p  = yaw;
    double yawd_p = yawd;

    state[0] = px_p;
    state[1] = py_p;
    state[2] = v_p;
    state[3] = yaw_p;
    state[4] = yawd_p;
}

/**
* Predicts sigma points, the state, and the state covariance matrix.
* @param {double} delta_t the change in time (in seconds) between the last
* measurement and this one.
*/
void UKF::Prediction(double delta_t, int modelInd) {
    /*****************************************************************************
   *  Initialize model parameters
   ****************************************************************************/
    double std_yawdd, std_a;
    MatrixXd x_(x_cv_.rows(), 1);
    MatrixXd P_(P_cv_.rows(),P_cv_.cols());
    MatrixXd Xsig_pred_(Xsig_pred_cv_.rows(), Xsig_pred_cv_.cols());
    if(modelInd == 0){
        x_ = x_cv_.col(0);
        P_ = P_cv_;
        Xsig_pred_ = Xsig_pred_cv_;
        std_yawdd = std_cv_yawdd_;
        std_a     = std_a_cv_;
    }
    else if(modelInd == 1){
        x_ = x_ctrv_.col(0);
        P_ = P_ctrv_;
        Xsig_pred_ = Xsig_pred_ctrv_;
        std_yawdd = std_ctrv_yawdd_;
        std_a     = std_a_ctrv_;
    }
    else{
        x_ = x_rm_.col(0);
        P_ = P_rm_;
        Xsig_pred_ = Xsig_pred_rm_;
        std_yawdd = std_rm_yawdd_;
        std_a     = std_a_rm_;
    }

    /*****************************************************************************
    *  Generate Sigma Points
    ****************************************************************************/
    //calculate square root of P
    MatrixXd A = P_.llt().matrixL();

    /*****************************************************************************
    *  Augment Sigma Points
    ****************************************************************************/
    //create augmented mean vector
    VectorXd x_aug = VectorXd(n_aug_);

    //create augmented state covariance
    MatrixXd P_aug = MatrixXd(n_aug_, n_aug_);

    //create sigma point matrix
    MatrixXd Xsig_aug = MatrixXd(n_aug_, 2 * n_aug_ + 1);

    //create augmented mean state
    x_aug.head(5) = x_;
    x_aug(5) = 0;
    x_aug(6) = 0;

    //create augmented covariance matrix
    P_aug.fill(0.0);
    P_aug.topLeftCorner(5, 5) = P_;
    P_aug(5, 5) = std_a*std_a;
    P_aug(6, 6) = std_yawdd*std_yawdd;

    //create square root matrix
    MatrixXd L = P_aug.llt().matrixL();

    //create augmented sigma points
    Xsig_aug.col(0) = x_aug;
//    if(modelInd == 2) cout<< "x state: "<<endl<<x_rm_<<endl;
//    if(modelInd == 2) cout<< "aug x state: "<<endl<<x_aug<<endl;
    for (int i = 0; i< n_aug_; i++)
    {
        Xsig_aug.col(i + 1) = x_aug + sqrt(lambda_aug_ + n_aug_) * L.col(i);
        Xsig_aug.col(i + 1 + n_aug_) = x_aug - sqrt(lambda_aug_ + n_aug_) * L.col(i);
    }
//    if(modelInd == 2) cout<< "aug sigma x points: "<<endl<<Xsig_aug<<endl;

    /*****************************************************************************
    *  Predict Sigma Points
    ****************************************************************************/
    //predict sigma points
    for (int i = 0; i < 2 * n_aug_ + 1; i++)
    {
        //extract values for better readability
        double p_x      = Xsig_aug(0, i);
        double p_y      = Xsig_aug(1, i);
        double v        = Xsig_aug(2, i);
        double yaw      = Xsig_aug(3, i);
        double yawd     = Xsig_aug(4, i);
        double nu_a     = Xsig_aug(5, i);
        double nu_yawdd = Xsig_aug(6, i);

        vector<double> state(5);
        if(modelInd == 0)        Cv(p_x, p_y, v, yaw, yawd, nu_a, nu_yawdd, delta_t, state);
        else if(modelInd == 1) Ctrv(p_x, p_y, v, yaw, yawd, nu_a, nu_yawdd, delta_t, state);
        else           randomMotion(p_x, p_y, v, yaw, yawd, nu_a, nu_yawdd, delta_t, state);

        //write predicted sigma point into right column
        Xsig_pred_(0, i) = state[0];
        Xsig_pred_(1, i) = state[1];
        Xsig_pred_(2, i) = state[2];
        Xsig_pred_(3, i) = state[3];
        Xsig_pred_(4, i) = state[4];
    }
//    if(modelInd == 2) cout<< "predicted sigma x points: "<<endl<<Xsig_pred_<<endl;

    /*****************************************************************************
    *  Convert Predicted Sigma Points to Mean/Covariance
    ****************************************************************************/
    //predicted state mean
    x_.fill(0.0);
    for (int i = 0; i < 2 * n_aug_ + 1; i++) {  //iterate over sigma points
        x_ = x_ + weights_(i) * Xsig_pred_.col(i);
    }

    while (x_(3)> M_PI) x_(3) -= 2.*M_PI;
    while (x_(3)<-M_PI) x_(3) += 2.*M_PI;
    //predicted state covariance matrix
    P_.fill(0.0);
    for (int i = 0; i < 2 * n_aug_ + 1; i++) {  //iterate over sigma points
        // state difference
        VectorXd x_diff = Xsig_pred_.col(i) - x_;
        //angle normalization
        while (x_diff(3)> M_PI) x_diff(3) -= 2.*M_PI;
        while (x_diff(3)<-M_PI) x_diff(3) += 2.*M_PI;
        P_ = P_ + weights_(i) * x_diff * x_diff.transpose();
    }
    P_ = P_ + Q_;

    /*****************************************************************************
    *  Update model parameters
    ****************************************************************************/
    if(modelInd == 0){
        x_cv_.col(0) = x_;
        P_cv_ = P_;
        Xsig_pred_cv_ = Xsig_pred_;
    }
    else if(modelInd == 1){
        x_ctrv_.col(0) = x_;
        P_ctrv_ = P_;
        Xsig_pred_ctrv_ = Xsig_pred_;
    }
    else{
        x_rm_.col(0) = x_;
        P_rm_ = P_;
        Xsig_pred_rm_ = Xsig_pred_;
    }

	ROS_DEBUG_NAMED("pred", "(pred, %d, %d, %f, %f)", debug_pred_id, modelInd, x_(0), x_(1));
}

/**
* Updates the state and the state covariance matrix using a laser measurement.
* @param {MeasurementPackage} meas_package
*/
void UKF::UpdateLidar(int modelInd) {
    // TODO refactoring
    /*****************************************************************************
   *  Initialize model parameters
   ****************************************************************************/
    VectorXd x(x_cv_.rows());
    MatrixXd P(P_cv_.rows(),P_cv_.cols());
    MatrixXd Xsig_pred(Xsig_pred_cv_.rows(), Xsig_pred_cv_.cols());
    if(modelInd == 0){
        x = x_cv_.col(0);
        P = P_cv_;
        Xsig_pred = Xsig_pred_cv_;
    }
    else if(modelInd == 1){
        x = x_ctrv_.col(0);
        P = P_ctrv_;
        Xsig_pred = Xsig_pred_ctrv_;
    }
    else{
        x = x_rm_.col(0);
        P = P_rm_;
        Xsig_pred = Xsig_pred_rm_;
    }

//    count_++;
    //extract measurement as VectorXd
    // VectorXd z = meas_package.raw_measurements_;
    //set measurement dimension, lidar can measure p_x and p_y
    int n_z = 2;

    //create matrix for sigma points in measurement space
    MatrixXd Zsig = MatrixXd(n_z, 2 * n_aug_ + 1);

    //transform sigma points into measurement space
    for (int i = 0; i < 2 * n_aug_ + 1; i++) {  //2n+1 simga points

        // extract values for better readibility
        double p_x = Xsig_pred(0, i);
        double p_y = Xsig_pred(1, i);

        // measurement model
        Zsig(0, i) = p_x;
        Zsig(1, i) = p_y;
    }

    //mean predicted measurement
    VectorXd z_pred = VectorXd(n_z);
    z_pred.fill(0.0);
    for (int i = 0; i < 2 * n_aug_ + 1; i++) {
        z_pred = z_pred + weights_(i) * Zsig.col(i);
    }

    //measurement covariance matrix S
    MatrixXd S = MatrixXd(n_z, n_z);
    S.fill(0.0);
    for (int i = 0; i < 2 * n_aug_ + 1; i++) {  //2n+1 simga points
        //residual
        VectorXd z_diff = Zsig.col(i) - z_pred;
        S = S + weights_(i) * z_diff * z_diff.transpose();
    }

    //add measurement noise covariance matrix
    MatrixXd R = MatrixXd(n_z, n_z);
    R << std_laspx_*std_laspx_, 0,
            0, std_laspy_*std_laspy_;
    S = S + R;

    //create matrix for cross correlation Tc
    MatrixXd Tc = MatrixXd(n_x_, n_z);

    /*****************************************************************************
    *  UKF Update for Lidar
    ****************************************************************************/
    //calculate cross correlation matrix
    Tc.fill(0.0);
    for (int i = 0; i < 2 * n_aug_ + 1; i++) {  //2n+1 simga points

        //residual
        VectorXd z_diff = Zsig.col(i) - z_pred;
        // state difference
        VectorXd x_diff = Xsig_pred.col(i) - x;
		cropAngle(x_diff(3));
        Tc = Tc + weights_(i) * x_diff * z_diff.transpose();
    }

    //Kalman gain K;
    MatrixXd K = Tc * S.inverse();

    // //residual
    // VectorXd z_diff = z - z_pred;

    // //update state mean and covariance matrix
    // x = x + K * z_diff;
    // P = P - K*S*K.transpose();

    // while (x(3)> M_PI) x(3) -= 2.*M_PI;
    // while (x(3)<-M_PI) x(3) += 2.*M_PI;
    /*****************************************************************************
    *  Update model parameters
    ****************************************************************************/

    if(modelInd == 0){
        x_cv_.col(0)  = x;
        P_cv_         = P;
        Xsig_pred_cv_ = Xsig_pred;
        zPredCVl_     = z_pred;
        lS_cv_        = S;
        K_cv_         = K;
    }
    else if(modelInd == 1){
        x_ctrv_.col(0)  = x;
        P_ctrv_         = P;
        Xsig_pred_ctrv_ = Xsig_pred;
        zPredCTRVl_     = z_pred;
        lS_ctrv_        = S;
        K_ctrv_         = K;
    }
    else{
        x_rm_.col(0)    = x;
        P_rm_           = P;
        Xsig_pred_rm_   = Xsig_pred;
        zPredRMl_       = z_pred;
        lS_rm_          = S;
        K_rm_           = K;
    }
}

void UKF::singleUpdate(VectorXd z, int modelInd){
    VectorXd z_pred;
    MatrixXd S, x_, P_, K;
    if(modelInd == 0){
        z_pred = zPredCVl_;
        S      = lS_cv_;
        x_     = x_cv_;
        P_     = P_cv_;
        K      = K_cv_;
    }
    else if(modelInd == 1){
        z_pred = zPredCTRVl_;
        S      = lS_ctrv_;
        x_     = x_ctrv_;
        P_     = P_ctrv_;
        K      = K_ctrv_;
    }
    else{
        z_pred = zPredRMl_;
        S      = lS_rm_;
        x_     = x_rm_;
        P_     = P_rm_;
        K      = K_rm_;
    }

    //residual
    VectorXd z_diff = z - z_pred;

    //calculate NIS
    NIS_laser_ = z_diff.transpose() * S.inverse() * z_diff;

	ROS_DEBUG_NAMED("ego", "nis: (%d, %f)", modelInd, NIS_laser_);
    //update state mean and covariance matrix
    x_ = x_ + K * z_diff;
    P_ = P_ - K*S*K.transpose();

    while (x_(3)> M_PI) x_(3) -= 2.*M_PI;
    while (x_(3)<-M_PI) x_(3) += 2.*M_PI;

    /*****************************************************************************
    *  Update model parameters
    ****************************************************************************/
    if(modelInd == 0){
        x_cv_.col(0)  = x_;
        P_cv_         = P_;
//        NISvals_laser_cv_ << NIS_laser_ << endl;
//        cout << "cv nis: "<< NIS_laser_ << endl;
    }
    else if(modelInd == 1){
        x_ctrv_.col(0)  = x_;
        P_ctrv_         = P_;
//        NISvals_laser_ctrv_ << NIS_laser_ << endl;
//        cout << "ctrv nis: "<< NIS_laser_ << endl;
    }
    else{
        x_rm_.col(0)    = x_;
        P_rm_           = P_;
//        NISvals_laser_rm_ << NIS_laser_ << endl;
//        cout << "x rm after update: "<<endl<<x_<<endl<<endl;
    }
}

void UKF::predictTraj(double predTime, const vector<vector<double>>& egoTraj)
{
	int preStep = int(predTime / pred_dt_);
	trajState_.clear();
	trajState_.reserve(preStep + 1);

	double mode[3] = {modeProbCV_, modeProbCTRV_, modeProbRM_};
	int modelInd = max_element(mode, mode+3) - mode;
	
	//ROS_DEBUG_NAMED("traj", "target %d mode (%f, %f, %f)", box_id_, modeProbCV_, modeProbCTRV_, modeProbRM_);
	
	double p_x      = x_merge_(0);
	double p_y      = x_merge_(1);
	double v        = x_merge_(2);
	double yaw      = x_merge_(3);
	double yawd     = x_merge_(4);
	double nu_a     = 0;
	double nu_yawdd = 0;

	vector<double> state(5);
	for(int i=0; i<5; i++) 	state[i] = x_merge_(i);
	trajState_.emplace_back(state);

	int ne = egoTraj.size();
	vector<vector<double>> eIncState;
	if(ne)
	{
		for(int i=1; i<egoTraj.size(); i++)
		{
			vector<double> tmp(2);
			tmp[0] = egoTraj[i][0] - egoTraj[0][0];
			tmp[1] = egoTraj[i][1] - egoTraj[0][1];
			eIncState.emplace_back(std::move(tmp));	
		}
	}

	for(int i=0; i<preStep; i++){
        if(modelInd == 0) 	 Cv(p_x, p_y, v, yaw, yawd, nu_a, nu_yawdd, pred_dt_, state);
        else if(modelInd == 1) 	Ctrv(p_x, p_y, v, yaw, yawd, nu_a, nu_yawdd, pred_dt_, state);
        else 	 randomMotion(p_x, p_y, v, yaw, yawd, nu_a, nu_yawdd, pred_dt_, state);
		
		p_x = state[0];
		p_y = state[1];
		v  	= state[2];
		yaw = state[3];
		yawd= state[4];
		
		if(ne){
			state[0] -= eIncState[i][0];
			state[1] -= eIncState[i][1];
		}	
		trajState_.emplace_back(state);
		ROS_DEBUG_NAMED("traj", "target %d mode (%d) traj (%f, %f)", box_id_,  modelInd, p_x, p_y);
	}

}

void UKF::predictTraj(double predTime, const Vector3d& z)
{
	int preStep = int(predTime / pred_dt_);
	trajState_.clear();
	trajState_.reserve(preStep + 1);

	double mode[3] = {modeProbCV_, modeProbCTRV_, modeProbRM_};
	int modelInd = max_element(mode, mode+3) - mode;
	ROS_DEBUG_NAMED("ego", "max mode(%d) (%f, %f, %f)", modelInd, modeProbCV_, modeProbCTRV_, modeProbRM_);
	// ROS_DEBUG_NAMED("ego", "preT yaw(%f)", z(2));

	double p_x      = z(0);
	double p_y      = z(1);
	double v        = 0;
	double yaw      = z(2);
	double yawd     = 0;
	double nu_a     = 0;
	double nu_yawdd = 0;

    if(modelInd == 0){
        v = x_cv_(2);
        yawd = x_cv_(4);
    }else if(modelInd == 1)
    {
        v = x_ctrv_(2);
        yawd = x_ctrv_(4);
    }else{
        v = x_rm_(2);
        yawd = x_rm_(4);
    }

	vector<double> state(5);
	state[0] = p_x;
    state[1] = p_y;
    state[2] = v;
    state[3] = yaw;
    state[4] = yawd;
	trajState_.emplace_back(state);

	for(int i=0; i<preStep; i++){
        if(modelInd == 0) 	 Cv(p_x, p_y, v, yaw, yawd, nu_a, nu_yawdd, pred_dt_, state);
        else if(modelInd == 1) 	Ctrv(p_x, p_y, v, yaw, yawd, nu_a, nu_yawdd, pred_dt_, state);
        else 	 randomMotion(p_x, p_y, v, yaw, yawd, nu_a, nu_yawdd, pred_dt_, state);
		
		p_x = state[0];
		p_y = state[1];
		v  	= state[2];
		yaw = state[3];
		yawd= state[4];
		
		trajState_.emplace_back(state);
		//ROS_DEBUG_NAMED("traj", "target %d mode (%d) traj (%f, %f)", box_id_,  modelInd, p_x, p_y);
	}

}





